#include "assist_land.h"
#include <iostream>
#include <thread>
#include "sift_api.h"
#include "ekf.h"


using namespace std;
using namespace cv;


#define SBUF_SIZE 512
// the longth of the external marker long edge is approximately 0.8 meters
#define EXTERNAL_LONG_EDGE (0.85f)
// the longth of the internal marker long edge is approximately 0.13 meters
#define INTERNAL_LONG_EDGE (0.10f)

// the longth of the alternate external marker long edge is approximately 0.6 meters
#define ALTERNATE_EXTERNAL_LONG_EDGE   (0.6f)

// the longth of the alternat internal marker long edge is approximately 0.12 meters
#define ALTERNATE_INTERNAL_LONG_EDGE   (0.12f)

#define IS_LAND_QR_OK_NUM		  5

#define FLIGHT_BEARING_LOCK_NUM   5

#define EKF_INIT_POSERROR_NUM	  5

#define USE_ALTERNATE_LAND_QR_HEIGHT  (10.0f)

#define START_CAMERA_SCAN_MODE		(8.0f)

#define CAMERA_SCAN_GIMBAL_ANGLE	(-60.0f)  //range: from -90 to -45

#define GIMBAL_ROTATE_SPEED_ANGLE	(10.0f)

#define LOITER_COPTER_FOR_ASSIST_LAND_INIT_BATTERYIFO	12   // 12%

#define START_CONTROL_FLIGHT_HEIGHT (80.0f)

#define CHANGE_ZOOM_TO_WIDTH_HEIGHT (7.0f)

#define LIVE_STREAM_FRAME_RATE 25

// angle unit: degree
#define FINISH_YAW_CONTROL_ANGLE   (2.0f)

// distance unit: m
#define FINISH_XY_CONTROL_DISTANCE (0.05f)

#define SET_Z_VELOCITY_ZERO_XY_DISTANCE (1.0f)

#define IDENTIFY_INTERAL_QR_HEIGHT  (3.0f)

#define CAMARE_TO_FLIGHT_CENTER_DISTANCE  (0.10f)

#define CIRCULAR_BUFFER_SIZE								(64 * 1024)

//the height threshold of change the camera zoom
#define CAMERA_ZOOM_HEIGHT_THRESH0LD_HIGHEST	(60.0f)

#define CAMERA_ZOOM_HEIGHT_THRESH0LD_HIGHER	(40.0f)

#define CAMERA_ZOOM_HEIGHT_THRESH0LD_HIGH	(20.0f)

#define CAMERA_ZOOM_HEIGHT_THRESH0LD_MIDDLE	(10.0f)

#define LOW_PASS_FILTER_ALPHA (0.3f)   //rang 0.0f- 1.0f
#define REMAIN_AVOID_DISTACE	(1.0f) // unit:m

#define CANCLE_ASSIST_LAND
//#define ALTERNATE_MARKER
#define DOUBLE_GIMBAL_CAMARE_TO_FLIGHT_CENTER_DISTANCE	(0.10f)

#define NO_HEART_RETURN_TIME		5
#define PARKING_APRON_ISOK_CHECK_HEIGHT		(20.0f)
#define USB_BLUK_MAY_BE_ERROR_HEIGHT		(20.0f)

#define FIXED_RTK_NEED_ASSIST_LAND_DISTANCE	(0.3f)
#define FIXED_RTK_NEED_ASSIST_LAND_YAWERROR	(25.0f)


enum measureDimUpdateState{
	measureDimUpdateX = 0,
	measureDimUpdateY = 1
};

enum obstaclsDirectionState{
	frontObstacles = 0,		// 315 - 360 and 0 -45 degree
	rightObstacles = 1,		// 45 - 135 degree
	backObstacles = 2,		// 135 - 225 degree
	leftObstacles = 3		// 225 - 315 degree
};

enum avoidWorkState
{
	notWorking = 0,
	working = 1
};


hbAssistLand::hbAssistLand() :
	m_djiFlightHandler(NULL),
	m_assistLandModeRunFlag(false),
	m_flightLiveViewFpvUpdateFlag(false),
	m_flightLiveViewMainUpdateFlag(false),
	m_flightLiveViewViceUpdateFlag(false),
	m_flightLiveViewTopUpdateFlag(false),
	m_haveFlightControlAuthorityFlag(false),
	m_positionErrorUpdateFlag(false),
	m_currentCameraUse(DJI_LIVEVIEW_CAMERA_SOURCE_DEFAULT),
	m_ekfInitSuccess(false),
	m_startCameraScanMode(false),
	m_siftHanderStatus(siftMarkerNone),
	m_needAlignCameraYaw(false),
	m_goToAlternateLandPoint(false),
	m_sumOfNoData(0),
	m_haveFixedRtkControlFlag(false)
{
}


hbAssistLand::~hbAssistLand()
{

}


static void communicationWithMsdkCallBack(linkProtocolType msg)
{
	cout << "communicationWithMsdkCallBack msg id:" << msg.m_msgid << endl;
}


void hbAssistLand::init(djiFlightHandler *handler)
{
	m_djiFlightHandler = handler;
	resetFlag();
	m_djiFlightHandler->registerMopChannelRecvCallback(communicationWithMsdkCallBack, AssistLandMode);
}

void hbAssistLand::sleepMs(uint32_t timeMs)
{
	m_djiFlightHandler->sleepMs(timeMs);
}




bool hbAssistLand::getAssistLandModeRunFlag()
{
	return m_assistLandModeRunFlag;
}


void hbAssistLand::setAssistLandModeRunFlag(bool flag)
{
	m_assistLandModeRunFlag = flag;
}


bool hbAssistLand::getAssistLandRunFunctionFlag()
{
	return m_assistLandRunFunctionFlag;
}

void hbAssistLand::setAssistLandRunFunctionFlag(bool flag)
{
	m_assistLandRunFunctionFlag = flag;
}



bool hbAssistLand::getSiftRunFunctionFlag()
{
	return m_siftRunFunctionFlag;
}

void hbAssistLand::setSiftRunFunctionFlag(bool flag)
{
	m_siftRunFunctionFlag = flag;
}




void hbAssistLand::setFlightLiveViewFpv(cv::Mat &image)
{
	unique_lock<mutex> ulock(m_flightLiveViewFpvLock);

	m_flightLiveViewFpv = image.clone();
	m_flightLiveViewFpvUpdateFlag = true;

	m_fpvImageFlightYaw = getFlightCurrentBearing();
	getGimbalAngles(m_fpvImageGimbalAngle);

	ulock.unlock();
}

void hbAssistLand::getFlightLiveViewFpv(cv::Mat &image, bool &updateFlag, double &flightYaw, T_DjiFcSubscriptionGimbalAngles &gimbalAngle)
{
	unique_lock<mutex> ulock(m_flightLiveViewFpvLock);

	image = m_flightLiveViewFpv.clone();
	updateFlag = m_flightLiveViewFpvUpdateFlag;

	m_flightLiveViewFpvUpdateFlag = false;
	flightYaw = m_fpvImageFlightYaw;
	memcpy(&gimbalAngle, &m_fpvImageGimbalAngle, sizeof(T_DjiFcSubscriptionGimbalAngles));

	ulock.unlock();
}

void hbAssistLand::setFlightLiveViewMain(cv::Mat &image)
{
	unique_lock<mutex> ulock(m_flightLiveViewMainLock);

	m_flightLiveViewMain = image.clone();
	m_flightLiveViewMainUpdateFlag = true;
	m_sumOfNoData = 0;

	m_mainImageFlightYaw = getFlightCurrentBearing();
	getGimbalAngles(m_mainImageGimbalAngle);

	ulock.unlock();

	writeLogFile("setFlightLiveViewMain m_mainImageFlightYaw:%f, m_mainImageGimbalAngle.x:%f, y:%f,z:%f\n",
										m_mainImageFlightYaw, m_mainImageGimbalAngle.x, m_mainImageGimbalAngle.y, m_mainImageGimbalAngle.z);
}

void hbAssistLand::getFlightLiveViewMain(cv::Mat &image,  bool &updateFlag, double &flightYaw, T_DjiFcSubscriptionGimbalAngles &gimbalAngle)
{
	unique_lock<mutex> ulock(m_flightLiveViewMainLock);

	image = m_flightLiveViewMain.clone();
	updateFlag = m_flightLiveViewMainUpdateFlag;
	m_flightLiveViewMainUpdateFlag = false;

	if(!updateFlag)
	{
		m_sumOfNoData++;
	}
	else
	{
		flightYaw = m_mainImageFlightYaw;
		memcpy(&gimbalAngle, &m_mainImageGimbalAngle, sizeof(T_DjiFcSubscriptionGimbalAngles));
	}

	ulock.unlock();
}

int hbAssistLand::getSumOfNoData()
{
	return m_sumOfNoData;
}



void hbAssistLand::setFlightLiveViewVice(cv::Mat &image)
{
	unique_lock<mutex> ulock(m_flightLiveViewViceLock);

	m_flightLiveViewVice = image.clone();
	m_flightLiveViewViceUpdateFlag = true;

	m_viceImageFlightYaw = getFlightCurrentBearing();
	getGimbalAngles(m_viceImageGimbalAngle);

	ulock.unlock();
}

void hbAssistLand::getFlightLiveViewVice(cv::Mat &image,  bool &updateFlag, double &flightYaw, T_DjiFcSubscriptionGimbalAngles &gimbalAngle)
{
	unique_lock<mutex> ulock(m_flightLiveViewViceLock);

	image = m_flightLiveViewVice.clone();
	updateFlag = m_flightLiveViewViceUpdateFlag;
	m_flightLiveViewViceUpdateFlag = false;

	flightYaw = m_viceImageFlightYaw;
	memcpy(&gimbalAngle, &m_viceImageGimbalAngle, sizeof(T_DjiFcSubscriptionGimbalAngles));

	ulock.unlock();
}

void hbAssistLand::setFlightLiveViewTop(cv::Mat &image)
{
	unique_lock<mutex> ulock(m_flightLiveViewTopLock);

	m_flightLiveViewTop = image.clone();
	m_flightLiveViewTopUpdateFlag = true;

	m_topImageFlightYaw = getFlightCurrentBearing();
	getGimbalAngles(m_topImageGimbalAngle);

	ulock.unlock();
}

void hbAssistLand::getFlightLiveViewTop(cv::Mat &image,  bool &updateFlag, double &flightYaw, T_DjiFcSubscriptionGimbalAngles &gimbalAngle)
{
	unique_lock<mutex> ulock(m_flightLiveViewTopLock);

	image = m_flightLiveViewTop.clone();
	updateFlag = m_flightLiveViewTopUpdateFlag;
	m_flightLiveViewTopUpdateFlag = false;

	flightYaw = m_topImageFlightYaw;
	memcpy(&gimbalAngle, &m_topImageGimbalAngle, sizeof(T_DjiFcSubscriptionGimbalAngles));

	ulock.unlock();
}


bool hbAssistLand::startFpvCameraStream(CameraImageCallback callback, E_DjiLiveViewCameraSource source)
{
	return m_djiFlightHandler->startFpvCameraStream(AssistLandMode, callback, &m_fpvName, source);
}

void hbAssistLand::stopFpvCameraStream(E_DjiLiveViewCameraSource source)
{
	m_djiFlightHandler->stopFpvCameraStream(AssistLandMode, source);
}

bool hbAssistLand::startMainCameraStream(CameraImageCallback callback, E_DjiLiveViewCameraSource source)
{
	return m_djiFlightHandler->startMainCameraStream(AssistLandMode, callback, &m_mainName, source);
}

void hbAssistLand::stopMainCameraStream(E_DjiLiveViewCameraSource source)
{
	m_djiFlightHandler->stopMainCameraStream(AssistLandMode, source);
}

bool hbAssistLand::startViceCameraStream(CameraImageCallback callback, E_DjiLiveViewCameraSource source)
{
	return m_djiFlightHandler->startViceCameraStream(AssistLandMode, callback, &m_viceName, source);
}

void hbAssistLand::stopViceCameraStream(E_DjiLiveViewCameraSource source)
{
	m_djiFlightHandler->stopViceCameraStream(AssistLandMode, source);
}

bool hbAssistLand::startTopCameraStream(CameraImageCallback callback, E_DjiLiveViewCameraSource source)
{
	return m_djiFlightHandler->startTopCameraStream(AssistLandMode, callback, &m_topName, source);
}

void hbAssistLand::stopTopCameraStream(E_DjiLiveViewCameraSource source)
{
	m_djiFlightHandler->stopTopCameraStream(AssistLandMode, source);
}


bool hbAssistLand::getDownStereoCameraParameters(T_DjiPerceptionCameraParameters &cameraParameters)
{
	bool ret;
	T_DjiPerceptionCameraParametersPacket packet = {0};
	ret = m_djiFlightHandler->getStereoCameraParameters(packet);
	if(ret)
	{
		for(int i=0; i<packet.directionNum; i++)
		{
			switch(packet.cameraParameters[i].direction)
			{
				case DJI_PERCEPTION_RECTIFY_DOWN:
				{
					memcpy(&cameraParameters, &packet.cameraParameters[i], sizeof(T_DjiPerceptionCameraParameters));
					break;
				}

				default:
					break;
			}
		}
	}

	return ret;
}


static int testPerceptionImageCount = 0;

static void DownPerceptionImageCallback(T_DjiPerceptionImageInfo imageInfo, uint8_t *imageRawBuffer, uint32_t bufferLen)
{
	if(imageInfo.rawInfo.direction == DJI_PERCEPTION_RECTIFY_DOWN)
	{
		assistLand.writeLogFile("DownPerceptionImageCallback index:%d, bpp:%u, width:%d, height:%d, dataId:%d, sequence:%d, dataType:%d, bufferLen:%d\n",
					imageInfo.rawInfo.index, imageInfo.rawInfo.bpp, imageInfo.rawInfo.width, imageInfo.rawInfo.height,
					imageInfo.dataId, imageInfo.sequence, imageInfo.dataType, bufferLen);


		uint32_t width = imageInfo.rawInfo.width;
		uint32_t height = imageInfo.rawInfo.height;

		Mat mat(height, width, CV_8U, imageRawBuffer);

#if 0 //for test
		char testImageName[30];
		memset(testImageName, 0, sizeof(testImageName) / sizeof(testImageName[0]));
		testPerceptionImageCount++;
		printf("testimagecount :%d\n", testPerceptionImageCount);
		sprintf(testImageName,"/testImage/%d.jpg",testPerceptionImageCount);
		imwrite(testImageName,mat);
#endif
	}
	else
	{
		assistLand.writeLogFile("DownPerceptionImageCallback  data is not down image!!!\n");
	}
}

static void FrontPerceptionImageCallback(T_DjiPerceptionImageInfo imageInfo, uint8_t *imageRawBuffer, uint32_t bufferLen)
{
	if(imageInfo.rawInfo.direction == DJI_PERCEPTION_RECTIFY_FRONT)
	{
		assistLand.writeLogFile("FrontPerceptionImageCallback index:%d, bpp:%u, width:%d, height:%d, dataId:%d, sequence:%d, dataType:%d, bufferLen:%d\n",
					imageInfo.rawInfo.index, imageInfo.rawInfo.bpp, imageInfo.rawInfo.width, imageInfo.rawInfo.height,
					imageInfo.dataId, imageInfo.sequence, imageInfo.dataType, bufferLen);


		uint32_t width = imageInfo.rawInfo.width;
		uint32_t height = imageInfo.rawInfo.height;

		Mat mat(height, width, CV_8U, imageRawBuffer);

#if 0 //for test
		char testImageName[30];
		memset(testImageName, 0, sizeof(testImageName) / sizeof(testImageName[0]));
		testPerceptionImageCount++;
		printf("testimagecount :%d\n", testPerceptionImageCount);
		sprintf(testImageName,"/testImage/%d.jpg",testPerceptionImageCount);
		imwrite(testImageName,mat);
#endif
	}
	else
	{
		assistLand.writeLogFile("FrontPerceptionImageCallback  data is not down image!!!\n");
	}
}


bool hbAssistLand::subscribeDownPerceptionImage()
{
	return m_djiFlightHandler->subscribePerceptionImage(DJI_PERCEPTION_RECTIFY_DOWN, DownPerceptionImageCallback);
}

bool hbAssistLand::unsubscribeDownPerceptionImage()
{
	return m_djiFlightHandler->unsubscribePerceptionImage(DJI_PERCEPTION_RECTIFY_DOWN);
}

bool hbAssistLand::getFrontStereoCameraParameters(T_DjiPerceptionCameraParameters &cameraParameters)
{
	bool ret;
	T_DjiPerceptionCameraParametersPacket packet = {0};
	ret = m_djiFlightHandler->getStereoCameraParameters(packet);
	if(ret)
	{
		for(int i=0; i<packet.directionNum; i++)
		{
			switch(packet.cameraParameters[i].direction)
			{
				case DJI_PERCEPTION_RECTIFY_FRONT:
				{
					memcpy(&cameraParameters, &packet.cameraParameters[i], sizeof(T_DjiPerceptionCameraParameters));
					break;
				}

				default:
					break;
			}
		}
	}

	return ret;
}

bool hbAssistLand::subscribeFrontPerceptionImage()
{
	return m_djiFlightHandler->subscribePerceptionImage(DJI_PERCEPTION_RECTIFY_FRONT, FrontPerceptionImageCallback);
}

bool hbAssistLand::unsubscribeFrontPerceptionImage()
{
	return m_djiFlightHandler->unsubscribePerceptionImage(DJI_PERCEPTION_RECTIFY_FRONT);
}


E_DjiLiveViewCameraSource hbAssistLand::getCurrentCameraUse()
{
	return m_currentCameraUse;
}

void hbAssistLand::setCurrentCameraUse(E_DjiLiveViewCameraSource type)
{
	m_currentCameraUse = type;
}



void hbAssistLand::writeLogFile(const char * format, ...)
{
	char sbuf[SBUF_SIZE];
	va_list args;
	va_start (args, format);
	vsnprintf (sbuf,SBUF_SIZE,format, args);
	va_end (args);

	m_djiFlightHandler->writeLogFile("%s", sbuf);
}


void hbAssistLand::getGimbalAngles(T_DjiFcSubscriptionGimbalAngles &pry, T_DjiDataTimestamp &timestamp)
{
	m_djiFlightHandler->getGimbalAngles(pry, timestamp);
}

void hbAssistLand::getGimbalAngles(T_DjiFcSubscriptionGimbalAngles &pry)
{
	T_DjiDataTimestamp timestamp;
	m_djiFlightHandler->getGimbalAngles(pry, timestamp);
}


double hbAssistLand::getGimbalAnglesYaw()
{
	T_DjiFcSubscriptionGimbalAngles gimbalAngles;
	T_DjiDataTimestamp timestamp;

	//x->pitch, y->roll, z->yaw
	m_djiFlightHandler->getGimbalAngles(gimbalAngles, timestamp);

	return gimbalAngles.z;
}

void hbAssistLand::computeMoveBearing(Point imageCenter, vector<Point> rect, int inOrEx, double &positionErr, double &rotationAngle, enum siftMarkerType type)
{
	//col error, x is col
	int errX = rect[rect.size()-1].x - imageCenter.x;

	//row error, y is row
	int errY = rect[rect.size()-1].y - imageCenter.y;

	//atan2 scale from - pi to pi
	double angle = atan2(fabsf(errY), fabsf(errX)) * (180.0f / DJI_PI);

	if(errX > 0 && errY < 0)
	{
		//one area
		angle = 90 - angle;
	}
	else if(errX > 0 && errY > 0)
	{
		//two area
		angle += 90 ;
	}
	else if(errX < 0 && errY > 0)
	{
		//three area
		angle = 270 - angle;
	}
	else if(errX < 0 && errY < 0)
	{
		//four area
		angle += 270;
	}

	positionErr = sqrt(pow(errX,2) + pow(errY, 2));
	rotationAngle = angle;

	float widthEdge, heightEdge;
	widthEdge = sqrt(pow(rect[1].x - rect[0].x, 2) + pow(rect[1].y - rect[0].y, 2));
	heightEdge = sqrt(pow(rect[2].x - rect[1].x, 2) + pow(rect[2].y - rect[1].y, 2));

	float ratio;
	float realLongth = 0.0f;

	switch(inOrEx)
	{
		case ExternalQrCode:
		{
			if(type !=siftAlternateLandMarker)
			{
				realLongth = EXTERNAL_LONG_EDGE;
			}
			else
			{
				realLongth = ALTERNATE_EXTERNAL_LONG_EDGE;
			}
			break;
		}
		case InternalQrCode:
		{
			if(type !=siftAlternateLandMarker)
			{
				realLongth = INTERNAL_LONG_EDGE;
			}
			else
			{
				realLongth = ALTERNATE_INTERNAL_LONG_EDGE;
			}
			break;
		}
	}

	ratio =  widthEdge > heightEdge ? realLongth / widthEdge : realLongth / heightEdge;

	positionErr *= ratio;
}



void hbAssistLand::computeCorrectBearing(const vector<float> data, double kMeasRadius, double &returnAngle)
{
	int indexMaxNum;
	int indexMax;
	int internCount;

	indexMaxNum = 0;
	//search the center point
	for(int i=0; i<data.size(); i++)
	{
		float tmp = data[i];
		internCount = 0;

		for(int j=0; j<data.size(); j++)
		{
			double error;
			error = data[j] - tmp;

			error = (float)fmod((float)fmod((error + 180), 360) + 360, 360) - 180;

			if(fabsf(error) < kMeasRadius)
			{
				internCount++;
			}
		}

		if(internCount > indexMaxNum)
		{
			indexMaxNum = internCount;
			indexMax = i;
		}
	}

	float center;
	float distanceSum;

	distanceSum = 0;
	center = data[indexMax];
	internCount = 0;

	for(int i=0; i<data.size(); i++)
	{
		double error;
		error = data[i] - center;
		error = (float)fmod((float)fmod((error + 180), 360) + 360, 360) - 180;

		if(fabsf(error) < kMeasRadius)
		{
			distanceSum += error;
			internCount++;
		}
	}

	returnAngle = center + distanceSum / (float)internCount;
}



double hbAssistLand::getFlightCurrentBearing()
{
	T_DjiFcSubscriptionQuaternion quaternion;
	T_DjiDataTimestamp messageTimestamp;
	m_djiFlightHandler->getFlightQuat(quaternion, messageTimestamp);

	double roll,pitch, yaw;
	m_djiFlightHandler->getRollPitchYawFromQuaternion(quaternion, roll, pitch, yaw);

	return yaw;
}


double hbAssistLand::getFlightCurrentHeigh()
{
	T_DjiDataTimestamp messageTimestamp;
	T_DjiFcSubscriptionAltitudeFused height;
	m_djiFlightHandler->getAltitudeFused(height, messageTimestamp);
	T_DjiFcSubscriptionAltitudeOfHomePoint homeAlt;
	m_djiFlightHandler->getAltitudeHomePoint(homeAlt, messageTimestamp);

	return height - homeAlt;
}

//fusion height range about 0.2m to 11m
double hbAssistLand::getCurrentHeightFusion()
{
	T_DjiFcSubscriptionHeightFusion height;
	T_DjiDataTimestamp messageTimestamp;
	m_djiFlightHandler->getHeightFusion(height, messageTimestamp);

	return height;
}

bool hbAssistLand::getEkfInitSuccessFlag()
{
	return m_ekfInitSuccess;
}

void hbAssistLand::setEkfInitSuccessFlag(bool ekfInitFlag)
{
	m_ekfInitSuccess = ekfInitFlag;
}

bool hbAssistLand::getNeedAlignCameraYaw()
{
	return m_needAlignCameraYaw;
}

void hbAssistLand::setNeedAlignCameraYaw(bool isNeed)
{
	m_needAlignCameraYaw = isNeed;
}

void hbAssistLand::setGoToAlternateLandPoint(bool status)
{
	m_goToAlternateLandPoint = status;
}


bool hbAssistLand::getStartCameraScanModeFlag()
{
	return m_startCameraScanMode;
}

void hbAssistLand::setStartCameraScanModeFlag(bool scanFlag)
{
	m_startCameraScanMode = scanFlag;
}


void hbAssistLand::setSiftHanderStatus(enum siftMarkerType &status)
{
	m_siftHanderStatus = status;
}

enum siftMarkerType hbAssistLand::getSiftHanderStatus()
{
	return m_siftHanderStatus;
}

bool hbAssistLand::obtainJoyStickCtrlAuthority()
{
	bool ret;
	if(m_haveFlightControlAuthorityFlag)
	{
		return true;
	}

	ret = m_djiFlightHandler->obtainJoyStickCtrlAuthority(AssistLandMode);

	if(!ret)
	{
		writeLogFile("hbAssistLand obtainJoyStickCtrlAuthority err\n");
		return false;
	}
	m_djiFlightHandler->setAssistLandSystemStatus(controlFlighLand);
	m_haveFlightControlAuthorityFlag = true;
	return true;
}


void hbAssistLand::releaseJoyStickCtrlAuthority()
{
	if(!m_haveFlightControlAuthorityFlag)
	{
		return;
	}

	m_haveFlightControlAuthorityFlag = false;
	m_djiFlightHandler->releaseJoyStickCtrlAuthority(AssistLandMode);
	m_djiFlightHandler->setAssistLandSystemStatus(monitorFlightState);
}

bool hbAssistLand::sendVelocityControlToCopter(T_DjiVector3f velocity,uint32_t timeoutMs,double yawRate)
{
	return m_djiFlightHandler->sendVelocityControlToCopter(AssistLandMode,velocity, timeoutMs, yawRate);
}


bool hbAssistLand::controlCopterTurnOffMotors()
{
	return m_djiFlightHandler->controlCopterTurnOffMotors(AssistLandMode);
}


bool hbAssistLand::sendAutoForceLandCommandToCopter()
{
	return m_djiFlightHandler->sendAutoForceLandCommandToCopter(AssistLandMode);
}


bool hbAssistLand::sendLandPointToCopter(T_DjiFlightControllerHomeLocation homeLocation)
{
	return m_djiFlightHandler->sendLandPointToCopter(AssistLandMode, homeLocation);
}

bool hbAssistLand::sendStartGoHomeToCopter()
{
	return m_djiFlightHandler->sendStartGoHomeToCopter(AssistLandMode);
}

bool hbAssistLand::getHaveFlightControlAuthorityFlag()
{
	return m_haveFlightControlAuthorityFlag;
}


double hbAssistLand::wrap360(const double angle)
{
	return m_djiFlightHandler->wrap360(angle);
}


bool hbAssistLand::gimbalManagerSetModeAndReset(E_DjiGimbalResetMode resetMode)
{
	bool ret;
	ret = m_djiFlightHandler->gimbalManagerSetModeAndReset(DJI_MOUNT_POSITION_PAYLOAD_PORT_NO1, DJI_GIMBAL_MODE_YAW_FOLLOW, resetMode);

	if(!ret)
	{
		writeLogFile("hbAssistLand gimbalManagerSetModeAndReset err\n");
		return false;
	}

	return true;
}


bool hbAssistLand::gimbalManagerSetModeAndRestAndRotate()
{
	bool ret;
	T_DjiGimbalManagerRotation rotation;

	double currentYaw =  getFlightCurrentBearing();
	double gimbalYaw = getGimbalAnglesYaw();

	rotation.rotationMode = DJI_GIMBAL_ROTATION_MODE_RELATIVE_ANGLE;
	rotation.pitch = -90.0f;
	rotation.roll = 0.0f;
	rotation.yaw = 0.0f;
	rotation.time = 0.5f;

	ret = m_djiFlightHandler->gimbalManagerSetModeAndRestAndRotate(DJI_MOUNT_POSITION_PAYLOAD_PORT_NO1, DJI_GIMBAL_MODE_YAW_FOLLOW, rotation);

	if(!ret)
	{
		writeLogFile("hbAssistLand gimbalManagerSetModeAndRestAndRotate err\n");
		return false;
	}

	return true;
}



bool hbAssistLand::gimbalManagersetModeAndRotate()
{
	bool ret;
	T_DjiGimbalManagerRotation rotation;

	double currentYaw =  getFlightCurrentBearing();
	double gimbalYaw = getGimbalAnglesYaw();

	rotation.rotationMode = DJI_GIMBAL_ROTATION_MODE_ABSOLUTE_ANGLE;
	rotation.pitch = -90.0f;
	rotation.roll = 0.0f;
	rotation.yaw = currentYaw;
	rotation.time = 0.5f;

	ret = m_djiFlightHandler->gimbalManagersetModeAndRotate(DJI_MOUNT_POSITION_PAYLOAD_PORT_NO1, DJI_GIMBAL_MODE_YAW_FOLLOW, rotation);

	if(!ret)
	{
		writeLogFile("hbAssistLand gimbalManagerSetModeAndRestAndRotate err\n");
		return false;
	}

	return true;
}


bool hbAssistLand::gimbalManagerRotate()
{
	bool ret;
	T_DjiGimbalManagerRotation rotation;

	double currentYaw =  getFlightCurrentBearing();
	double gimbalYaw = getGimbalAnglesYaw();

	rotation.rotationMode = DJI_GIMBAL_ROTATION_MODE_ABSOLUTE_ANGLE;
	rotation.pitch = -90.0f;
	rotation.roll = 0.0f;
	rotation.yaw = gimbalYaw;
	rotation.time = 0.5f;

	ret = m_djiFlightHandler->gimbalManagerRotate(DJI_MOUNT_POSITION_PAYLOAD_PORT_NO1, rotation);

	if(!ret)
	{
		writeLogFile("hbAssistLand gimbalManagerRotate err\n");
		return false;
	}

	return true;
}

bool hbAssistLand::gimbalManagerRotate(double rotateAngle)
{
	bool ret;
	T_DjiGimbalManagerRotation rotation;

	rotation.rotationMode = DJI_GIMBAL_ROTATION_MODE_ABSOLUTE_ANGLE;
	rotation.pitch = -90.0f;
	rotation.roll = 0.0f;
	rotation.yaw = rotateAngle;
	rotation.time = 0.5f;

	ret = m_djiFlightHandler->gimbalManagerRotate(DJI_MOUNT_POSITION_PAYLOAD_PORT_NO1, rotation);

	if(!ret)
	{
		writeLogFile("hbAssistLand gimbalManagerRotate err\n");
		return false;
	}

	return true;
}


bool hbAssistLand::cameraScanModeRotateAbsoluteAngle(double rotateRelativeAngle)
{
	bool ret;
	T_DjiGimbalManagerRotation rotation;

	double currentYaw =  getFlightCurrentBearing();
	double gimbalYaw = getGimbalAnglesYaw();

	rotation.rotationMode = DJI_GIMBAL_ROTATION_MODE_ABSOLUTE_ANGLE;
	rotation.pitch = CAMERA_SCAN_GIMBAL_ANGLE;
	rotation.roll = 0.0f;
	rotation.yaw = gimbalYaw + rotateRelativeAngle;
	rotation.time = 0.5f;

	ret = m_djiFlightHandler->gimbalManagerRotate(DJI_MOUNT_POSITION_PAYLOAD_PORT_NO1, rotation);

	if(!ret)
	{
		writeLogFile("hbAssistLand gimbalManagerRotate err\n");
		return false;
	}

	return true;
}


bool hbAssistLand::cameraScanModeRotateSpeed(double rotateRelativeAngle)
{
	bool ret;
	T_DjiGimbalManagerRotation rotation;

	rotation.rotationMode = DJI_GIMBAL_ROTATION_MODE_SPEED;
	rotation.pitch = 0.0f;
	rotation.roll = 0.0f;
	rotation.yaw = rotateRelativeAngle;
	rotation.time = 0.5f;

	ret = m_djiFlightHandler->gimbalManagerRotate(DJI_MOUNT_POSITION_PAYLOAD_PORT_NO1, rotation);

	if(!ret)
	{
		writeLogFile("hbAssistLand gimbalManagerRotate err\n");
		return false;
	}

	return true;
}



bool hbAssistLand::setH20CameraZoom(float multiplier)
{
	return m_djiFlightHandler->setCameraZoom(DJI_MOUNT_POSITION_PAYLOAD_PORT_NO1, multiplier);
}

double hbAssistLand::getFlightInHomeBearing()
{
	return m_flightInHomeBearing;
}


bool hbAssistLand::sendCancelLandCommandtoCopter()
{
	return m_djiFlightHandler->sendCancelLandCommandtoCopter(AssistLandMode);
}

bool hbAssistLand::sendAutoLandCommandToCopter()
{
	return m_djiFlightHandler->sendAutoLandCommandToCopter(AssistLandMode);
}

T_DjiFcSubscriptionFlightStatus hbAssistLand::getFlightCurrentStatus()
{
	return m_flightCurrentStatus;
}


double hbAssistLand::getDifferenceGimbalYawAndFlightYaw()
{
	return m_differenceGimbalYawAndFlightYaw;
}

T_DjiFcSubscriptionGimbalControlMode hbAssistLand::getCurrentGimbalMode()
{
	return m_currentGimbalMode;
}


T_DjiFcSubscriptionRtkYawInfo hbAssistLand::getRtkYawInfo()
{
	T_DjiFcSubscriptionRtkYawInfo rtkYawStatus;
	T_DjiDataTimestamp messageTimestamp;
	m_djiFlightHandler->getRtkYawInfo(rtkYawStatus, messageTimestamp);

	return rtkYawStatus;
}


void hbAssistLand::getFlightVelocity(T_DjiFcSubscriptionVelocity &velocity, T_DjiDataTimestamp &timestamp)
{
	m_djiFlightHandler->getFlightVelocity(velocity, timestamp);
}

void hbAssistLand::getAvoidData(T_DjiFcSubscriptionAvoidData &avoidData, T_DjiDataTimestamp &timestamp)
{
	m_djiFlightHandler->getAvoidData(avoidData, timestamp);
}

bool hbAssistLand::getHomoBearingLockFlag()
{
	return m_homoBearingLockFlag;
}

void hbAssistLand::setHomoBearingLockFlag(double computeLandBearing)
{
	m_computeLandBearing = computeLandBearing;
	m_homoBearingLockFlag = true;
}

double hbAssistLand::getComputeLandBearing()
{
	return m_computeLandBearing;
}

uint8_t hbAssistLand::getBatteryInfoPercentage()
{
	T_DjiFcSubscriptionWholeBatteryInfo batteryInfo;
	T_DjiDataTimestamp timeStamp;
	m_djiFlightHandler->getBatteryInfo(batteryInfo, timeStamp);

	return batteryInfo.percentage;
}





bool hbAssistLand::setFlightControlModeByVelocity()
{
	return m_djiFlightHandler->setFlightControlModeByVelocity(AssistLandMode);
}


bool hbAssistLand::velocityControlToCopter(T_DjiVector3f velocity, double yawRate)
{
	return m_djiFlightHandler->velocityControlToCopter(AssistLandMode, velocity, yawRate);
}

void hbAssistLand::resetFlag()
{
	m_assistLandRunFunctionFlag = false;
	m_siftRunFunctionFlag = false;
	m_homoBearingLockFlag = false;
}


void hbAssistLand::resetTranslationSum()
{
	unique_lock<mutex> ulock(m_translationSumLock);
	memset(&m_translationSum, 0, sizeof(Vector3f));
	ulock.unlock();
}


void hbAssistLand::setTranslationSum(Vector3f translationAdd)
{
	unique_lock<mutex> ulock(m_translationSumLock);
	m_translationSum.x += translationAdd.x;
	m_translationSum.y += translationAdd.y;
	m_translationSum.z += translationAdd.z;
	ulock.unlock();
}

void hbAssistLand::getTranslationSum(Vector3f &translation)
{
	unique_lock<mutex> ulock(m_translationSumLock);
	memcpy(&translation, &m_translationSum, sizeof(Vector3f));
	ulock.unlock();

	writeLogFile("getTranslationSum x:%f, y:%f, z:%f\n", m_translationSum.x, m_translationSum.y, m_translationSum.z);
}



void hbAssistLand::setPositionError(Vector3f positionErr)
{
	unique_lock<mutex> ulock(m_positionErrorLock);
	memcpy(&m_positionError, &positionErr, sizeof(Vector3f));
	m_positionErrorUpdateFlag = true;
	ulock.unlock();

	writeLogFile("setPositionError x:%f, y:%f, z:%f\n", m_positionError.x, m_positionError.y, m_positionError.z);
}

void hbAssistLand::getPositionError(Vector3f &positionErr, bool &updateFlag)
{
	unique_lock<mutex> ulock(m_positionErrorLock);
	updateFlag = m_positionErrorUpdateFlag;
	m_positionErrorUpdateFlag = false;
	memcpy(&positionErr, &m_positionError, sizeof(Vector3f));
	ulock.unlock();
}


bool hbAssistLand::getLandPoint(T_DjiFlightControllerHomeLocation &landPoint)
{
	bool ret = false;
	double latitude;
	double longitude;
	ret =  m_djiFlightHandler->getLandPoint(latitude, longitude);
	landPoint.latitude = latitude * DJI_PI / 180;
	landPoint.longitude = longitude * DJI_PI / 180;
	return ret;
}


bool hbAssistLand::getCanLandFlag()
{
	return m_djiFlightHandler->getCanLandFlag();
}

int hbAssistLand::getNoHeartCount()
{
	return m_djiFlightHandler->getNoHeartCount();
}

bool hbAssistLand::goToAlternateLandingPoint()
{
	if(!obtainJoyStickCtrlAuthority())
	{
		writeLogFile("goToAlternateLandingPoint obtainJoyStickCtrlAuthority error\n");
		return false;
	}

	// if flight status is landing, we first cancal land, then set alternate LandingPoint.
	if(m_flightCurrentMode == DJI_FC_SUBSCRIPTION_DISPLAY_MODE_AUTO_LANDING)
	{
		if(!sendCancelLandCommandtoCopter())
		{
			writeLogFile("goToAlternateLandingPoint sendCancelLandCommandtoCopter error\n");
			return false;
		}
	}

	T_DjiFlightControllerHomeLocation landPoint;
	if(getLandPoint(landPoint))
	{
		sendLandPointToCopter(landPoint);
		if(sendStartGoHomeToCopter())
		{
			setGoToAlternateLandPoint(true);
			setSiftRunFunctionFlag(false);
			setAssistLandRunFunctionFlag(false);
			stopMainCameraStream(getCurrentCameraUse());
			releaseJoyStickCtrlAuthority();
			return true;
		}
	}

	return false;
}



void hbAssistLand::sendCancelLandToMsdk()
{
	m_djiFlightHandler->sendCancelLandToMsdk();
}

static void obtainFpvCameraStreamCallback(CameraRGBImage img, void *userData)
{
	string name = string(reinterpret_cast<char *>(userData));

	 Mat mat(img.height, img.width, CV_8UC3, img.rawData.data(), img.width * 3);

	 assistLand.setFlightLiveViewFpv(mat);
}

static int testImageCount = 0;

static void obtainMainCameraStreamCallback(CameraRGBImage img, void *userData)
{
	string name = string(reinterpret_cast<char *>(userData));

	 Mat mat(img.height, img.width, CV_8UC3, img.rawData.data(), img.width * 3);

	 assistLand.setFlightLiveViewMain(mat);
#if 0 //for test
	 char testImageName[30];
	 memset(testImageName, 0, sizeof(testImageName) / sizeof(testImageName[0]));
	 testImageCount++;
	 printf("testimagecount :%d\n", testImageCount);
	 sprintf(testImageName,"/testImage/%d.jpg",testImageCount);
	 imwrite(testImageName,mat);
#endif
}


static void obtainTopCameraStreamCallback(CameraRGBImage img, void *userData)
{
	string name = string(reinterpret_cast<char *>(userData));

	 Mat mat(img.height, img.width, CV_8UC3, img.rawData.data(), img.width * 3);

	 assistLand.setFlightLiveViewTop(mat);
}


static void obtainViceCameraStreamCallback(CameraRGBImage img, void *userData)
{
	string name = string(reinterpret_cast<char *>(userData));

	 Mat mat(img.height, img.width, CV_8UC3, img.rawData.data(), img.width * 3);

	 assistLand.setFlightLiveViewVice(mat);
}


bool siftHandlerMarker(vector<KeyPoint> &markerExKp, Mat &markerExDes, vector<KeyPoint> &markerInKp, Mat &markerInDes, enum siftMarkerType type)
{
	bool ret;
	Mat markerEx, markerIn;

	switch(type)
	{
		case siftLandMarker:
		{
			markerEx = imread("/home/forlinx/hbSource/external_marker.png", IMREAD_GRAYSCALE);
			markerIn = imread("/home/forlinx/hbSource/internal_marker.png", IMREAD_GRAYSCALE);
			break;
		}
		case siftAlternateLandMarker:
		{
			markerEx = imread("/home/forlinx/hbSource/alternate_external_marker.png", IMREAD_GRAYSCALE);
			markerIn = imread("/home/forlinx/hbSource/alternate_internal_marker.png", IMREAD_GRAYSCALE);
			break;
		}
		default:
			return false;
	}

	ret = siftHandleImage(markerEx, markerExKp, markerExDes);

	if(!ret)
	{
		assistLand.writeLogFile("siftHandlerMarker  siftHandleImage external marker err\n");
		assistLand.setAssistLandRunFunctionFlag(false);
		return false;
	}

	ret = siftHandleImage(markerIn, markerInKp, markerInDes);

	if(!ret)
	{
		assistLand.writeLogFile("siftHandlerMarker  siftHandleImage internal marker err\n");
		assistLand.setAssistLandRunFunctionFlag(false);
		return false;
	}

	return true;
}


void algorithmTest(Mat &im, vector<Point> rect, double rationAngle, int inOrEx)
{
	vector<Point> lines;
	for(int i=0; i<rect.size() - 1; i++)
	{
		lines.push_back(rect[i]);
	}

	Point center;
	center.x = rect[rect.size()-1].x;
	center.y = rect[rect.size()-1].y;

	vector<vector<Point> > pts;
	pts.push_back(lines);

	//Scalar color format is gbr
	drawContours(im, pts, -1, Scalar(0,0,255), 1, 8);

	line(im, Point(center.x - 10, center.y),Point(center.x + 10, center.y), Scalar(0,0,255));
	line(im, Point(center.x, center.y - 10), Point(center.x, center.y + 10), Scalar(0,0,255));

	imshow("video", im);
	waitKey(30);
}

void hbAssistLand::controlH20CamerZoom(enum cameraZoomControlSizeState &cameraZoomState)
{
	double flightCurrentHeight = getFlightCurrentHeigh();
	if(flightCurrentHeight > CAMERA_ZOOM_HEIGHT_THRESH0LD_HIGHEST)
	{
		if(cameraZoomState != cameraZoomHeightHighest)
		{
			if(setH20CameraZoom(cameraZoomHeightHighest))
			{
				cameraZoomState = cameraZoomHeightHighest;
			}
		}
	}
	else if(flightCurrentHeight > CAMERA_ZOOM_HEIGHT_THRESH0LD_HIGHER)
	{
		if(cameraZoomState != cameraZoomHeightHigher)
		{
			if(setH20CameraZoom(cameraZoomHeightHigher))
			{
				cameraZoomState = cameraZoomHeightHigher;
			}
		}
	}
	else if(flightCurrentHeight > CAMERA_ZOOM_HEIGHT_THRESH0LD_HIGH)
	{
		if(cameraZoomState != cameraZoomHeightHigh)
		{
			if(setH20CameraZoom(cameraZoomHeightHigh))
			{
				cameraZoomState = cameraZoomHeightHigh;
			}
		}
	}
	else if(flightCurrentHeight > CAMERA_ZOOM_HEIGHT_THRESH0LD_MIDDLE)
	{
		if(cameraZoomState != cameraZoomHeightMiddle)
		{
			if(setH20CameraZoom(cameraZoomHeightMiddle))
			{
				cameraZoomState = cameraZoomHeightMiddle;
			}
		}
	}
	else
	{
		if(cameraZoomState != cameraZoomHeightLow)
		{
			if(setH20CameraZoom(cameraZoomHeightLow))
			{
				cameraZoomState = cameraZoomHeightLow;
			}
		}
	}
}

bool hbAssistLand::isDoubleGimbal()
{
	return m_djiFlightHandler->isDoubleGimbal();
}

void *siftRun(void *arg)
{
	bool ret;

	assistLand.setSiftRunFunctionFlag(true);

	//first, gimbal reset to align with the flight head, this need two second
	//second, rotate the pitch from 0 degree to -90 dgree with the relative angle mothod.
	assistLand.gimbalManagerSetModeAndReset();

	double differenceGimbalYawAndFlightYaw;
	//recode the diffrence of between gimbal yaw with flight yaw, this data can be use for setting the flight landing bearing.
	differenceGimbalYawAndFlightYaw = assistLand.getFlightCurrentBearing() - assistLand.getGimbalAnglesYaw();

	enum siftMarkerType siftHanderStatus;
	siftHanderStatus = siftMarkerNone;
	int siftCount = 0;

	vector<KeyPoint> markerExKp, markerInKp;
	Mat markerExDes, markerInDes;

	//handler the external marker and internal marker by sift
	siftHandlerMarker(markerExKp, markerExDes, markerInKp, markerInDes, siftLandMarker);

#ifdef ALTERNATE_MARKER
	vector<KeyPoint> alternateMarkerExKp, alternateMarkerInKp;
	Mat alternateMarkerExDes, alternateMarkerInDes;

	siftHandlerMarker(alternateMarkerExKp, alternateMarkerExDes, alternateMarkerInKp, alternateMarkerInDes, siftAlternateLandMarker);
#endif

	Mat im;
	Mat imGray;
	vector<float> guessBearing;
	double intervalMs = (1.0f / (float)LIVE_STREAM_FRAME_RATE) * 1e3;
	int delay = 2;

	//camera scan mode
	bool scanOver = false;
	double lastGimbalYaw;
	double currentGimbalAngle;
	assistLand.setStartCameraScanModeFlag(false);
	assistLand.setEkfInitSuccessFlag(false);
	assistLand.setNeedAlignCameraYaw(false);

	while(assistLand.getSiftRunFunctionFlag())
	{
		//wait for System scheduling
		assistLand.sleepMs(delay);
		assistLand.resetTranslationSum();

		//get new liveview
		bool updateFlag;
		double imageFlightYaw;
		T_DjiFcSubscriptionGimbalAngles imageGimbalAngle;
		assistLand.getFlightLiveViewMain(im, updateFlag, imageFlightYaw, imageGimbalAngle);

		if(!updateFlag)
		{
			delay = 2;
			continue;
		}

		if((imageGimbalAngle.x < -91.0f || imageGimbalAngle.x > -89.0f) && (!assistLand.getStartCameraScanModeFlag()))
		{
			//confirm the gimbal camera head is down
			assistLand.gimbalManagerRotate();
			assistLand.writeLogFile("siftRun gimbalManagerRotate-90 x:%f, y:%f, z:%f\n", imageGimbalAngle.x, imageGimbalAngle.y, imageGimbalAngle.z);
			delay = intervalMs;
			continue;
		}

		if(assistLand.getStartCameraScanModeFlag())
		{
			//camera scan angle CAMERA_SCAN_GIMBAL_ANGLE
			if((imageGimbalAngle.x < CAMERA_SCAN_GIMBAL_ANGLE - 1 || imageGimbalAngle.x > CAMERA_SCAN_GIMBAL_ANGLE + 1))
			{
				assistLand.cameraScanModeRotateAbsoluteAngle(0.0f);
				assistLand.writeLogFile("siftRun gimbalManagerRotate-45 x:%f, y:%f, z:%f\n", imageGimbalAngle.x, imageGimbalAngle.y, imageGimbalAngle.z);
				continue;
			}
			else
			{
				//if(siftHanderStatus == camerScanModeSearch)
				{
					lastGimbalYaw = currentGimbalAngle;
					currentGimbalAngle = assistLand.getGimbalAnglesYaw();

					if(fabs(currentGimbalAngle - lastGimbalYaw) < 1.0f)
					{
						scanOver = true;
						//recovery the camera status(down and first angle)
						assistLand.gimbalManagerSetModeAndReset();
						assistLand.sleepMs(2000);
						differenceGimbalYawAndFlightYaw = assistLand.getFlightCurrentBearing() - assistLand.getGimbalAnglesYaw();
						assistLand.setNeedAlignCameraYaw(false);
						//set scan mode false
						assistLand.setStartCameraScanModeFlag(false);
						assistLand.writeLogFile("siftRun camera-scan-360, but not successfull.\n");
						#ifdef CANCLE_ASSIST_LAND
						//search marker failed to switch auto land for groundstation control copter to other landpoint.
						assistLand.writeLogFile("siftRun search marker failed to switch auto land for groundstation control copter to other landpoint.\n");
						assistLand.goToAlternateLandingPoint();
						#endif
						continue;
					}

					//Rotate 360 degrees clockwise
					assistLand.cameraScanModeRotateSpeed(GIMBAL_ROTATE_SPEED_ANGLE);
				}
			}
		}

		if(assistLand.getEkfInitSuccessFlag() && assistLand.getStartCameraScanModeFlag())
		{
			//reset yaw
			assistLand.gimbalManagerSetModeAndReset();
			assistLand.sleepMs(2000);
			differenceGimbalYawAndFlightYaw = assistLand.getFlightCurrentBearing() - assistLand.getGimbalAnglesYaw();
			assistLand.setNeedAlignCameraYaw(false);
			//set scan mode false
			assistLand.setStartCameraScanModeFlag(false);
			scanOver = true;
			assistLand.writeLogFile("siftRun ekfinit-success-and-close-camera-scan.\n");
			continue;
		}

		cvtColor(im, imGray, CV_RGB2GRAY);

		if(imGray.empty())
		{
			assistLand.writeLogFile("siftRun  imgray is empty  .....err\n");
			delay = intervalMs;
			continue;
		}

		vector<Point> rect;
		double rationAngle;
		int inOrEx = 0;
		int externalGoodMatchesMinNum;
		int internalGoodMatchesMinNum;
		int returnGoodMatches = 0;

		double flightCurrentHeight = assistLand.getFlightCurrentHeigh();

		//whether start the camera scan for ekf init
		if(flightCurrentHeight < START_CAMERA_SCAN_MODE && !assistLand.getEkfInitSuccessFlag()
			&& !scanOver && !assistLand.getStartCameraScanModeFlag())
		{
			assistLand.writeLogFile("siftRun start-camera-scan-mode\n");
			siftHanderStatus = camerScanModeSearch;
			assistLand.setNeedAlignCameraYaw(true);
			assistLand.setSiftHanderStatus(siftHanderStatus);
			assistLand.setStartCameraScanModeFlag(true);
			assistLand.cameraScanModeRotateAbsoluteAngle(-90);
			//Rotate pitch CAMERA_SCAN_GIMBAL_ANGLE, then rotate yaw 90, need 2 sconds.
			assistLand.sleepMs(2000);
			currentGimbalAngle = assistLand.getGimbalAnglesYaw();
			assistLand.cameraScanModeRotateSpeed(GIMBAL_ROTATE_SPEED_ANGLE);
			continue;
		}

#ifdef ALTERNATE_MARKER
		if(flightCurrentHeight < USE_ALTERNATE_LAND_QR_HEIGHT)
		{
			if(siftHanderStatus == siftMarkerNone)
			{
				siftHanderStatus = siftAlternateLandMarker;
				assistLand.setSiftHanderStatus(siftHanderStatus);
			}
		}
#endif
		inOrEx = AllQrCode;
		externalGoodMatchesMinNum = 20;
		internalGoodMatchesMinNum = 18;

		if(assistLand.getStartCameraScanModeFlag())
		{
			externalGoodMatchesMinNum = 15;
			internalGoodMatchesMinNum = 18;
		}

		// record the start time
		std::chrono::steady_clock::time_point startTime = std::chrono::steady_clock::now();

		switch(siftHanderStatus)
		{
			case siftMarkerNone:
			case siftLandMarker:
			{
				// land marker sift
				ret = computeLandPosition(imGray, markerExKp, markerExDes, markerInKp, markerInDes, rect, rationAngle,
										inOrEx, externalGoodMatchesMinNum, internalGoodMatchesMinNum, returnGoodMatches);
				break;
			}

			case siftAlternateLandMarker:
			{
				#ifdef ALTERNATE_MARKER
				// alternate land marker sift
				ret = computeLandPosition(imGray, alternateMarkerExKp, alternateMarkerExDes, alternateMarkerInKp, alternateMarkerInDes, rect, rationAngle,
										inOrEx, externalGoodMatchesMinNum, internalGoodMatchesMinNum, returnGoodMatches);

				#endif
				break;
			}

			case camerScanModeSearch:
			{
				//land marker sift
				ret = computeLandPosition(imGray, markerExKp, markerExDes, markerInKp, markerInDes, rect, rationAngle,
										inOrEx, externalGoodMatchesMinNum, internalGoodMatchesMinNum, returnGoodMatches, false);
				if(ret)
				{
					siftHanderStatus = siftLandMarker;
					assistLand.setSiftHanderStatus(siftHanderStatus);
				}
				else
				{
					#ifdef ALTERNATE_MARKER
					// alternate land marker sift
					ret = computeLandPosition(imGray, alternateMarkerExKp, alternateMarkerExDes, alternateMarkerInKp, alternateMarkerInDes, rect, rationAngle,
										inOrEx, externalGoodMatchesMinNum, internalGoodMatchesMinNum, returnGoodMatches, false);
					if(ret)
					{
						siftHanderStatus = siftAlternateLandMarker;
						assistLand.setSiftHanderStatus(siftHanderStatus);
					}
					#endif
				}
				break;
			}
		}

		//record the finish time
		std::chrono::steady_clock::time_point finishTime = std::chrono::steady_clock::now();

		//compute the  algorithm use time
		double usedTimeMs = std::chrono::duration_cast<std::chrono::duration<double>>(finishTime - startTime).count() * 1e3;
		assistLand.writeLogFile("siftRun usedTimeMs:%f\n", usedTimeMs);

		if(ret)
		{
			if(siftHanderStatus == siftMarkerNone)
			{
				siftCount++;
				if(siftCount > IS_LAND_QR_OK_NUM)
				{
					siftHanderStatus = siftLandMarker;
					assistLand.setSiftHanderStatus(siftHanderStatus);
				}
			}

			//compute computeLandBearing
			if(ExternalQrCode == inOrEx && !assistLand.getHomoBearingLockFlag())
			{
				double flightLandAngle;
				//the gimbal maybe not align with the flight head, so here use gimbal yaw add difference.
				flightLandAngle = imageGimbalAngle.z + rationAngle + differenceGimbalYawAndFlightYaw;
				flightLandAngle = assistLand.wrap360(flightLandAngle);

				guessBearing.push_back(flightLandAngle);

				if(guessBearing.size() > FLIGHT_BEARING_LOCK_NUM)
				{
					//identify the land bearing.
					double computeLandBearing;
					assistLand.computeCorrectBearing(guessBearing, 5, computeLandBearing);
					assistLand.setHomoBearingLockFlag(computeLandBearing);
					assistLand.writeLogFile("siftRun computeLandBearing:%f is locked\n", computeLandBearing);
				}
			}

			//image center
			Point imageCenter(imGray.cols / 2, imGray.rows / 2);
			double positionErr, rotation;
			assistLand.computeMoveBearing(imageCenter, rect, inOrEx, positionErr, rotation, siftHanderStatus);

			assistLand.writeLogFile("siftRun positionErr:%f, roation:%f, flightCurrentYaw:%f, gimbalAngles.x:%f, gimbalAngles.y:%f, gimbalAngles.z:%f\n",
								positionErr, rotation, imageFlightYaw, imageGimbalAngle.x, imageGimbalAngle.y, imageGimbalAngle.z);

			//according to the gimbalAngles compute the velocity bearing
			double positionBearing;

			//when the flight bearing lost rtk status, the flight yaw is bigger difference than gimbal yaw,
			//so velocity bearing use the gimbal yaw.
			positionBearing = imageGimbalAngle.z + rotation + differenceGimbalYawAndFlightYaw;

			positionBearing = assistLand.wrap360(positionBearing);

			Vector3f translation;
			assistLand.getTranslationSum(translation);

			Vector3f positionErrXYZ;

			//compute north position error
			positionErrXYZ.x = positionErr * cos(positionBearing * DJI_PI / 180) - translation.x;

			//compute east position error
			positionErrXYZ.y = positionErr * sin(positionBearing * DJI_PI / 180) - translation.y;

			positionErrXYZ.z = 0;

			//assistLand.writeLogFile("siftRun-pos-error1 x:%f, y:%f\n", positionErrXYZ.x, positionErrXYZ.y);

			//because the distance from the center of the external marker to the center of the interal marker is 0.12m 
			if(ExternalQrCode == inOrEx)
			{
				if(assistLand.getHomoBearingLockFlag())
				{
					double errorAngle = assistLand.getComputeLandBearing();
					positionErrXYZ.x += CAMARE_TO_FLIGHT_CENTER_DISTANCE * cos(errorAngle * DJI_PI / 180);
					positionErrXYZ.y += CAMARE_TO_FLIGHT_CENTER_DISTANCE * sin(errorAngle * DJI_PI / 180);
				}
			}

			if(assistLand.getHomoBearingLockFlag() && assistLand.isDoubleGimbal())
			{
				//h20 hang up port1 on the left bearing of m350 copter.
				double errorAngle = assistLand.getComputeLandBearing() - 90;
				positionErrXYZ.x += DOUBLE_GIMBAL_CAMARE_TO_FLIGHT_CENTER_DISTANCE * cos(errorAngle * DJI_PI / 180);
				positionErrXYZ.y += DOUBLE_GIMBAL_CAMARE_TO_FLIGHT_CENTER_DISTANCE * sin(errorAngle * DJI_PI / 180);
			}

			if(assistLand.getStartCameraScanModeFlag())
			{
				double radAngle = fabs(90 + CAMERA_SCAN_GIMBAL_ANGLE) * (DJI_PI / 180);
				double rotateAddDistance = flightCurrentHeight * tan(radAngle);
				double guessGimbalAngle;
				guessGimbalAngle = imageGimbalAngle.z + differenceGimbalYawAndFlightYaw;
				//tan(fabs(CAMERA_SCAN_GIMBAL_ANGLE)), so the ground distance is equal with flightCurrentHeight
				positionErrXYZ.x += rotateAddDistance * cos(guessGimbalAngle * DJI_PI / 180);
				positionErrXYZ.y += rotateAddDistance * sin(guessGimbalAngle * DJI_PI / 180);
			}

			assistLand.setPositionError(positionErrXYZ);

			//assistLand.writeLogFile("siftRun-pos-error2 x:%f, y:%f , siftRun positionBearing:%f, translation x:%f, y:%f\n",
				//positionErrXYZ.x, positionErrXYZ.y, positionBearing, translation.x, translation.y);
		}

		//compute the sleep time
		delay = intervalMs - usedTimeMs;
		delay = delay <= 0 ? 2 : delay;
	}

	assistLand.stopMainCameraStream(assistLand.getCurrentCameraUse());
	assistLand.gimbalManagerSetModeAndReset(DJI_GIMBAL_RESET_MODE_PITCH_AND_YAW);
	return NULL;
}


void ekfLinearFilter(double *u, double* x, double dt,double *Xdot, double* F)
{
	/* state prediction
     positionErrorSum -= (v_start + v_end) * dt / 2
     velocity  += v_start + a*dt
  	*/

	//u[0] is vx end, u[1] is vy end

  	//positionErrorSum .x xdot -->vx
	Xdot[0] = -(u[0] + x[2]) / 2.0f;
	//positionErrorSum .y xdot -->vy
	Xdot[1] = -(u[1] + x[3]) / 2.0f;

	//velocity .x xdot--->ax
	Xdot[2] = (u[0] - x[2]) / dt;

	//velocity .y xdot--->ay
	Xdot[3] = (u[1] - x[3]) / dt;

	/* F is the Jacobian of xdot wrt the state
		F[0*4+0] = d(Xdot[0]) / d(x[0]);
		F[0*4+1] = d(Xdot[0]) / d(x[1]);
		F[0*4+2] = d(Xdot[0]) / d(x[2]);
		F[0*4+3] = d(Xdot[0]) / d(x[3]);

		F[1*4+0] = d(Xdot[1]) / d(x[0]);
		F[1*4+1] = d(Xdot[1]) / d(x[1]);
		F[1*4+2] = d(Xdot[1]) / d(x[2]);
		F[1*4+3] = d(Xdot[1]) / d(x[3]);

		F[2*4+0] = d(Xdot[2]) / d(x[0]);
		F[2*4+1] = d(Xdot[2]) / d(x[1]);
		F[2*4+2] = d(Xdot[2]) / d(x[2]);
		F[2*4+3] = d(Xdot[2]) / d(x[3]);

		F[3*4+0] = d(Xdot[3]) / d(x[0]);
		F[3*4+1] = d(Xdot[3]) / d(x[1]);
		F[3*4+2] = d(Xdot[3]) / d(x[2]);
		F[3*4+3] = d(Xdot[3]) / d(x[3]);
	*/
	F[0*4+0] = 0.;
	F[0*4+1] = 0.;
	F[0*4+2] = -0.5f;
	F[0*4+3] = 0.;

	F[1*4+0] = 0.;
	F[1*4+1] = 0.;
	F[1*4+2] = 0.;
	F[1*4+3] = -0.5f;

	F[2*4+0] = 0.;
	F[2*4+1] = 0.;
	F[2*4+2] = -1/dt;
	F[2*4+3] = 0.;

	F[3*4+0] = 0.;
	F[3*4+1] = 0.;
	F[3*4+2] = 0.;
	F[3*4+3] = -1/dt;
}


void ekfLinearMeasure(double *y, double* err, double*x, double *H, int measureDimUpdate)
{
	/* err = measure - estimate
	 	H is the jacobian of the measure wrt X
	 	H[0] = d(y[0])/d(x[0]);
	 	H[1] = d(y[0])/d(x[1]);
	 	H[2] = d(y[0])/d(x[2]);
	 	H[3] = d(y[0])/d(x[3]);
	*/
	switch(measureDimUpdate)
	{
		case measureDimUpdateX:
		{
			*err = y[0] - x[0];
			H[0] = 1.;
			H[1] = 0.;
			H[2] = 0.;
			H[3] = 0.;
			break;
		}
		case measureDimUpdateY:
		{
			*err = y[0] - x[1];
			H[0] = 0.;
			H[1] = 1.;
			H[2] = 0.;
			H[3] = 0.;
			break;
		}
	}
}


float hbAssistLand::controlVelocityZ()
{
	double flightCurrentHeight = getFlightCurrentHeigh();
	float velocityZ = 0.0f;

	if(flightCurrentHeight > 30.0f)
	{
		//down speed set -1.5m/s
		velocityZ = -1.5f;
	}
	else if(flightCurrentHeight > 10.0f)
	{
		//down speed set -1m/s
		velocityZ = -1.0f;
	}
	else if(flightCurrentHeight > 5.0f)
	{
		//down speed set -1m/s
        velocityZ = -0.5f;
	}
	else
	{
		//down speed set -0.3m/s
		velocityZ = -0.3f;
	}

	if(m_startCameraScanMode)
	{
		if(getBatteryInfoPercentage() > LOITER_COPTER_FOR_ASSIST_LAND_INIT_BATTERYIFO)
		{
			//wait assist land init about 5s
			velocityZ = 0.0f;
		}
	}

	return velocityZ;
}

void hbAssistLand::controlVelocityXY(PID &txPid, PID &tyPid, Vector3f error, T_DjiVector3f &velocity)
{
	//compute the pid
	velocity.x = txPid.getPid(error.x);
	velocity.y = tyPid.getPid(error.y);
}

double hbAssistLand::controlYaw(PID &yawPid, double &angleError)
{
	double yawRate;
	if(m_homoBearingLockFlag)
	{
		double computeLandBearing, flightCurrentYaw, flightAngleErr;
		computeLandBearing = getComputeLandBearing();
		flightCurrentYaw = getFlightCurrentBearing();

		flightAngleErr = computeLandBearing - flightCurrentYaw;
		flightAngleErr = (float)fmod((float)fmod((flightAngleErr + 180), 360) + 360, 360) - 180;

		//compute the pid
		yawRate = yawPid.getPid(flightAngleErr);
		angleError = flightAngleErr;
	}
	else
	{
		yawRate = 0;
		yawPid.resetI();
		angleError = 0;
	}

	return yawRate;
}

bool hbAssistLand::getHaveFixedRtkControlFlag()
{
	return m_haveFixedRtkControlFlag;
}

void hbAssistLand::setHaveFixedRtkControlFlag(bool flag)
{
	m_haveFixedRtkControlFlag = flag;
}

T_DjiFcSubscriptionRtkPositionInfo hbAssistLand::getRtkPositionInfo()
{
	T_DjiFcSubscriptionRtkPositionInfo status;
	T_DjiDataTimestamp timestamp;
	m_djiFlightHandler->getRtkPositionInfo(status, timestamp);

	return status;
}


bool hbAssistLand::getIsFixBaseRTKStaus()
{
	return m_djiFlightHandler->getIsFixBaseRTKStaus();
}




bool hbAssistLand::whetherStartControlFlight(Vector3f error)
{
	//compute yaw error
	if(m_homoBearingLockFlag)
	{
		double computeLandBearing, flightCurrentYaw, flightAngleErr;
		computeLandBearing = getComputeLandBearing();
		flightCurrentYaw = getFlightCurrentBearing();

		flightAngleErr = computeLandBearing - flightCurrentYaw;
		flightAngleErr = (float)fmod((float)fmod((flightAngleErr + 180), 360) + 360, 360) - 180;

		if(fabs(flightAngleErr) > 3 * FINISH_YAW_CONTROL_ANGLE)
		{
			return true;
		}
	}

	//compute xy error
	float distanceError;
	distanceError = sqrt(pow(error.x,2) + pow(error.y, 2));
	if(distanceError > 3 * FINISH_XY_CONTROL_DISTANCE)
	{
		return true;
	}

	return false;
}


bool hbAssistLand::startControlFlight()
{
	if(0 != access("/home/forlinx/hbSource/external_marker.png", F_OK) 				||
		0 != access("/home/forlinx/hbSource/internal_marker.png", F_OK) 			||
		0 != access("/home/forlinx/hbSource/alternate_external_marker.png", F_OK) 	||
		0 != access("/home/forlinx/hbSource/alternate_internal_marker.png", F_OK))
	{
		writeLogFile("startControlFlight /home/forlinx/hbSource/external_marker.png may be not exist\n");
		return false;
	}


	if(!obtainJoyStickCtrlAuthority())
	{
		writeLogFile("startControlFlight obtainJoyStickCtrlAuthority error\n");
		return false;
	}

	if(!startMainCameraStream(obtainMainCameraStreamCallback, DJI_LIVEVIEW_CAMERA_SOURCE_H20_WIDE))
	{
		writeLogFile("startControlFlight startMainCameraStream err\n");
		releaseJoyStickCtrlAuthority();
	}
	else
	{
		setCurrentCameraUse(DJI_LIVEVIEW_CAMERA_SOURCE_H20_WIDE);
	}

	if(getIsFixBaseRTKStaus() == true)
	{
		m_haveFixedRtkControlFlag = true;
	}

	if(!m_haveFixedRtkControlFlag || getRtkPositionInfo() != DJI_FC_SUBSCRIPTION_POSITION_SOLUTION_PROPERTY_NARROW_INT)
	{
		if(!sendCancelLandCommandtoCopter())
		{
			writeLogFile("startControlFlight sendCancelLandCommandtoCopter err\n");
			stopMainCameraStream(getCurrentCameraUse());
			releaseJoyStickCtrlAuthority();
			return false;
		}
	}

	//send cancel land msg to msdk
	//sendCancelLandToMsdk();

	if(!setFlightControlModeByVelocity())
	{
		writeLogFile("startControlFlight setFlightControlModeByVelocity err\n");
		stopMainCameraStream(getCurrentCameraUse());
		releaseJoyStickCtrlAuthority();
		return false;
	}

	return true;
}

template <class T>
void lowPassFilter(T &output, T sample, float alpha)
{
	output += (sample - output) * alpha;
}


//Detecting obstacles in the forward direction
bool hbAssistLand::detecteObstaclesInTheForwardDirection(Vector3f targetPosition)
{
	bool ret;
	double targetDistance = sqrt(pow(targetPosition.x, 2) + pow(targetPosition.y, 2));
	targetDistance += REMAIN_AVOID_DISTACE;

	//through the target position calculate the movement direction.
	//atan2 scale from - pi to pi
	double moveAngle = atan2(targetPosition.y, targetPosition.x) * (180.0f / DJI_PI);
	moveAngle = wrap360(moveAngle);

	double frontAngle = getFlightCurrentBearing();
	double angleError = wrap360(moveAngle - frontAngle);

	int checkObstaclesDirection;
	if((315.0f < angleError && angleError <= 360.0f)  || (0.0f < angleError && angleError <= 45.0f))
	{
		checkObstaclesDirection = frontObstacles;
	}
	else if(45.0f < angleError && angleError <= 135.0f)
	{
		checkObstaclesDirection = rightObstacles;
	}
	else if(135.0f < angleError && angleError <= 225.0f)
	{
		checkObstaclesDirection = backObstacles;
	}
	else
	{
		checkObstaclesDirection = leftObstacles;
	}

	T_DjiFcSubscriptionAvoidData avoidData;
	T_DjiDataTimestamp timestamp;

	//get obstacles from flight
	getAvoidData(avoidData,timestamp);

	ret = false;
	//m350 Obstacle perception range: from 0.7m to 40m in the right, lift, front, back,
	//from 0.6m to 30m in the up and down
	switch(checkObstaclesDirection)
	{
		case frontObstacles:
		{
			if(avoidData.frontHealth == working  && (0.7f < avoidData.front && avoidData.front < 40.0f))
			{
				if(avoidData.front < targetDistance)
				{
					ret = true;
					writeLogFile("detecteObstaclesInTheForwardDirection front:%f, targetDistance:%f, moveAngle:%f, frontAngle:%f\n",
													avoidData.front, targetDistance, moveAngle, frontAngle);
				}
			}
			break;
		}

		case rightObstacles:
		{
			if(avoidData.rightHealth == working  && (0.7f < avoidData.right && avoidData.right < 40.0f))
			{
				if(avoidData.right < targetDistance)
				{
					ret = true;
					writeLogFile("detecteObstaclesInTheForwardDirection right:%f, targetDistance:%f ,moveAngle:%f, frontAngle:%f\n",
													avoidData.right, targetDistance, moveAngle, frontAngle);
				}
			}
			break;
		}

		case backObstacles:
		{
			if(avoidData.backHealth == working  && (0.7f < avoidData.back && avoidData.back < 40.0f))
			{
				if(avoidData.back < targetDistance)
				{
					ret = true;
					writeLogFile("detecteObstaclesInTheForwardDirection back:%f, targetDistance:%f, moveAngle:%f, frontAngle:%f\n",
													avoidData.back, targetDistance,  moveAngle, frontAngle);
				}
			}
			break;
		}

		case leftObstacles:
		{
			if(avoidData.leftHealth == working  && (0.7f < avoidData.left && avoidData.left < 40.0f))
			{
				if(avoidData.left < targetDistance)
				{
					ret = true;
					writeLogFile("detecteObstaclesInTheForwardDirection left:%f, targetDistance:%f, moveAngle:%f, frontAngle:%f\n",
													avoidData.left, targetDistance, moveAngle, frontAngle);
				}
			}
			break;
		}

		default:
				break;
	}

	return ret;
}

void *assistLandRun(void *arg)
{
	Vector3f translationXy;
	T_DjiFcSubscriptionVelocity latestVelocity;
	T_DjiDataTimestamp latestVelocityTimestamp;
	T_DjiFcSubscriptionVelocity currentVelocity;
	T_DjiDataTimestamp currentVelocityTimestamp;
	bool velocityUpdateFlag;

	double dt;
	std::chrono::steady_clock::time_point latestTime;
	std::chrono::steady_clock::time_point currentTime;

	Vector3f positionErrorSum;
	bool positionErrorSumInitFlag;
	positionErrorSumInitFlag = false;

	bool runControlFlightFlag;
	runControlFlightFlag = false;
	memset(&positionErrorSum, 0, sizeof(Vector3f));

	assistLand.setAssistLandRunFunctionFlag(true);

	//pid
	PID yawPid(1.0f, 0.1f, 0.01f, 500);
	PID txPid(2.5f, 0.25f, 0.02f, 2);
	PID tyPid(2.5f, 0.25f, 0.02f, 2);

	//ekf
	// initial X status: pos x, pos y, vel x, vel y
  	double X[4] = {0.0, 0.0, 0.0, 0.0};
	// initial state covariance matrix
  	double P[4*4] = {1.0, 0.0, 0.0, 0.0,
  					  0.0, 1.0, 0.0, 0.0,
  					  0.0, 0.0, 1.0, 0.0,
  					  0.0, 0.0, 0.0, 1.0};
	// model noise covariance matrix
	double Q[4*4]={1.5, 0.0, 0.0, 0.0,
				   0.0, 1.5, 0.0, 0.0,
				   0.0, 0.0, 1.5, 0.0,
				   0.0, 0.0, 0.0, 1.5};
	// measurement noise covariance matrix
	double R[1]={0.02};
	// measure: this is sift compute posError x y
  	double y[1];
	//command: this flight velocity x y
	double u[2];

	struct ekf_filter* filter;
  	filter = ekf_filter_new(4, 1, Q, R, ekfLinearFilter, ekfLinearMeasure);

	assistLand.getFlightVelocity(latestVelocity, latestVelocityTimestamp);
	latestTime = std::chrono::steady_clock::now();
	velocityUpdateFlag = false;

	vector<Vector3f> ekfInitPosError;

	T_DjiVector3f velocityOutput;
	double yawRateOutput = 0.0f;
	memset(&velocityOutput, 0, sizeof(T_DjiVector3f));

	while(assistLand.getAssistLandRunFunctionFlag())
	{
		//wait for System scheduling
		assistLand.sleepMs(2);

		//compute the xy translationSum
		assistLand.getFlightVelocity(currentVelocity, currentVelocityTimestamp);

		if(currentVelocityTimestamp.microsecond != latestVelocityTimestamp.microsecond)
		{
			currentTime = std::chrono::steady_clock::now();

			//get the dt, unit: s
			dt = std::chrono::duration_cast<std::chrono::duration<double>>(currentTime - latestTime).count();

			//compute the north and east translation: s = (v_start + v_end) * dt / 2
			translationXy.x = (currentVelocity.data.x  + latestVelocity.data.x) * dt / 2;
			translationXy.y = (currentVelocity.data.y  + latestVelocity.data.y) * dt / 2;
			translationXy.z = 0;

			//update latestTime and latestVelocity
			latestTime = currentTime;
			memcpy(&latestVelocity, &currentVelocity, sizeof(T_DjiFcSubscriptionVelocity));
			memcpy(&latestVelocityTimestamp, &currentVelocityTimestamp, sizeof(T_DjiDataTimestamp));

			//update the translation sum
			assistLand.setTranslationSum(translationXy);

			velocityUpdateFlag = true;
		}

		Vector3f positionErr;
		bool updateFlag;
		//get the position distance of sift computing.
		assistLand.getPositionError(positionErr, updateFlag);
		if(updateFlag)
		{
			if(!positionErrorSumInitFlag)
			{
				ekfInitPosError.push_back(positionErr);
				if(ekfInitPosError.size() > EKF_INIT_POSERROR_NUM ||
					(assistLand.getStartCameraScanModeFlag() && ekfInitPosError.size() > 0))
				{
					memset(&positionErr, 0, sizeof(Vector3f));
					for(int i=0; i<ekfInitPosError.size(); i++)
					{
						positionErr.x += ekfInitPosError[i].x;
						positionErr.y += ekfInitPosError[i].y;
					}

					positionErr.x /= (float)ekfInitPosError.size();
					positionErr.y /= (float)ekfInitPosError.size();

					//ekf init
					X[0] = positionErr.x;
					X[1] = positionErr.y;
					X[2] = currentVelocity.data.x;
					X[3] = currentVelocity.data.y;

					ekf_filter_reset(filter, X, P);

					positionErrorSum.x = X[0];
					positionErrorSum.y = X[1];

					assistLand.writeLogFile("assistLandRun-ekf-init x[0]:%f, x[1]:%f, x[2]:%f, x[3]:%f\n", X[0], X[1], X[2], X[3]);

					assistLand.setEkfInitSuccessFlag(true);

					positionErrorSumInitFlag = true;
					velocityUpdateFlag = false;
				}
			}
			else
			{
				//update x measure
				y[0] = positionErr.x;
				ekf_filter_update(filter, y, measureDimUpdateX);

				//update y measure
				y[0] = positionErr.y;
				ekf_filter_update(filter, y, measureDimUpdateY);

				//get ekf state
				ekf_filter_get_state(filter, X, P);

				positionErrorSum.x = X[0];
				positionErrorSum.y = X[1];
				//assistLand.writeLogFile("assistLandRun-ekf x[0]:%f, x[1]:%f, x[2]:%f, x[3]:%f, positionErr1 x:%f, y:%f, currentVelocity.x:%f, y%f\n",
							//X[0], X[1], X[2], X[3], positionErr.x, positionErr.y, currentVelocity.data.x, currentVelocity.data.y);
			}
		}


		T_DjiVector3f velocity;
		double yawRate;
		double angleError;
		angleError = 0.0f;

		//control yaw
		yawRate = assistLand.controlYaw(yawPid, angleError);

		//control z
		velocity.z = assistLand.controlVelocityZ();

		double xyDistance;
		xyDistance = 0.0f;
		if(positionErrorSumInitFlag && !assistLand.getNeedAlignCameraYaw())
		{
			if(velocityUpdateFlag && dt > 0)
			{
				velocityUpdateFlag = false;
				//update ekf predict
				u[0] = currentVelocity.data.x;
				u[1] = currentVelocity.data.y;

				ekf_filter_predict(filter, u, dt);
				ekf_filter_get_state(filter, X, P);

				positionErrorSum.x = X[0];
				positionErrorSum.y = X[1];
			}

			//control xy
			assistLand.controlVelocityXY(txPid, tyPid, positionErrorSum, velocity);

			//control z according to the distance of xy
			xyDistance = sqrt(pow(positionErrorSum.x, 2) + pow(positionErrorSum.y, 2));
			if(xyDistance > SET_Z_VELOCITY_ZERO_XY_DISTANCE)
			{
				if(assistLand.getBatteryInfoPercentage() > LOITER_COPTER_FOR_ASSIST_LAND_INIT_BATTERYIFO)
				{
					velocity.z = 0.0f;
				}
			}

			//Detecting obstacles in the forward direction: x is north(0 degree), y is east (90 degree)
			if(assistLand.detecteObstaclesInTheForwardDirection(positionErrorSum))
			{
				//handle the situation of having obstales
			}
		}
		else
		{
			velocity.x = 0.0f;
			velocity.y = 0.0f;
			txPid.resetI();
			tyPid.resetI();
		}

		if(assistLand.getIsFixBaseRTKStaus())
		{
			assistLand.writeLogFile("assistLandRun xyDistance:%f, angleError:%f, height:%f\n", xyDistance, angleError, assistLand.getFlightCurrentHeigh());
			if(((xyDistance > FIXED_RTK_NEED_ASSIST_LAND_DISTANCE
				|| angleError > FIXED_RTK_NEED_ASSIST_LAND_YAWERROR)
				&& assistLand.getFlightCurrentHeigh() < START_CAMERA_SCAN_MODE
				&& assistLand.getHaveFixedRtkControlFlag())
				|| assistLand.getRtkPositionInfo() != DJI_FC_SUBSCRIPTION_POSITION_SOLUTION_PROPERTY_NARROW_INT)
			{
				assistLand.writeLogFile("assistLandRun xyDistance:%f, angleError:%f\n", xyDistance, angleError);
				if(!assistLand.sendCancelLandCommandtoCopter())
				{
					//here failed, sorry, I don't know how to handler this stats.
					assistLand.writeLogFile("assistLandRun sendCancelLandCommandtoCopter err\n");
					//may be groundstation can hender.
					//break;
				}
				else
				{
					assistLand.setHaveFixedRtkControlFlag(false);
					velocity.x = 0.0f;
					velocity.y = 0.0f;
					txPid.resetI();
					tyPid.resetI();
					yawRate = 0;
					yawPid.resetI();
				}
			}
		}

		//low pass filter can make the plane run smooth
		lowPassFilter<float>(velocityOutput.x, velocity.x, LOW_PASS_FILTER_ALPHA);
		lowPassFilter<float>(velocityOutput.y, velocity.y, LOW_PASS_FILTER_ALPHA);
		lowPassFilter<float>(velocityOutput.z, velocity.z, LOW_PASS_FILTER_ALPHA);
		lowPassFilter<double>(yawRateOutput, yawRate, LOW_PASS_FILTER_ALPHA);

		assistLand.velocityControlToCopter(velocityOutput, yawRateOutput);

		//assistLand.writeLogFile("assistLandRun  distanceError x:%f, y:%f, controlvelocity:x:%f, y:%f, z:%f, yawRate:%f\n",
													//positionErrorSum.x, positionErrorSum.y, velocity.x, velocity.y, velocity.z, yawRate);

		if(assistLand.getFlightCurrentHeigh() < PARKING_APRON_ISOK_CHECK_HEIGHT
			&& (!assistLand.getCanLandFlag() || assistLand.getNoHeartCount() > NO_HEART_RETURN_TIME))
		{
			#ifdef CANCLE_ASSIST_LAND
			//parking apron have some problem to switch auto land for groundstation control copter to other landpoint.
			assistLand.writeLogFile("assistLandRun parking apron have some problem to switch auto land for groundstation control copter to other landpoint."
										"canLand:%u, no heartCount:%d\n", (short)assistLand.getCanLandFlag(), assistLand.getNoHeartCount());
			assistLand.goToAlternateLandingPoint();
			#endif
		}

		if(assistLand.getFlightCurrentHeigh() < USB_BLUK_MAY_BE_ERROR_HEIGHT
			&& (assistLand.getSumOfNoData() > 500 && !(assistLand.getIsFixBaseRTKStaus()
					&& assistLand.getRtkPositionInfo() == DJI_FC_SUBSCRIPTION_POSITION_SOLUTION_PROPERTY_NARROW_INT)))
		{
			assistLand.writeLogFile("assistLandRun usb bluk may be error, nodata:%d, FixRTKStatus:%u,rtkStatus:%d\n",
							assistLand.getSumOfNoData(), (short)assistLand.getIsFixBaseRTKStaus(), (int)assistLand.getRtkPositionInfo());
			assistLand.goToAlternateLandingPoint();
		}
	}

	if(assistLand.getHaveFlightControlAuthorityFlag())
	{
		assistLand.releaseJoyStickCtrlAuthority();
	}

	return NULL;
}



void hbAssistLand::assistLand()
{
	if(m_flightCurrentStatus != DJI_FC_SUBSCRIPTION_FLIGHT_STATUS_IN_AIR)
	{
		writeLogFile("status-error1\n");
		return;
	}

	if(getFlightCurrentHeigh() > START_CONTROL_FLIGHT_HEIGHT)
	{
		writeLogFile("height-error:%f\n", getFlightCurrentHeigh());
		return;
	}

	#ifdef CANCLE_ASSIST_LAND
	if(m_goToAlternateLandPoint)
	{
		return;
	}
	#endif

	if(m_assistLandRunFunctionFlag && m_siftRunFunctionFlag)
	{
		return;
	}
	else
	{
		m_assistLandRunFunctionFlag = false;
		m_siftRunFunctionFlag = false;
	}

	if(!startControlFlight())
	{
		goToAlternateLandingPoint();
		return;
	}

	m_djiFlightHandler->setAssistLandSystemStatus(controlFlighLand);

	int ret = pthread_create(&m_landRunTid, NULL, assistLandRun, NULL);
	if(0 != ret)
	{
		writeLogFile("assistLand assistLandRun pthread_create err\n");
		return;
	}

	ret = pthread_create(&m_siftRunTid, NULL, siftRun, NULL);
	if(0 != ret)
	{
		m_assistLandRunFunctionFlag = false;
		writeLogFile("assistLand siftRun pthread_create err\n");
		return;
	}
}

void hbAssistLand::run()
{

	if(NULL == m_djiFlightHandler)
	{
		return;
	}

	m_assistLandModeRunFlag = true;

	m_djiFlightHandler->setAssistLandSystemStatus(monitorFlightState);

	//about 5s for get cameraType.
	m_djiFlightHandler->getCameraType();

	while(m_assistLandModeRunFlag)
	{
		T_DjiDataTimestamp messageTimestamp;
		m_djiFlightHandler->getFlightMode(m_flightCurrentMode, messageTimestamp);
		m_djiFlightHandler->getFlightStatus(m_flightCurrentStatus, messageTimestamp);
		m_djiFlightHandler->getGimbalControlMode(m_currentGimbalMode, messageTimestamp);

		writeLogFile("assistland main sleep ok mode:%u, status:%u\n", m_flightCurrentMode, m_flightCurrentStatus);
		switch(m_flightCurrentStatus)
		{
			case DJI_FC_SUBSCRIPTION_FLIGHT_STATUS_STOPED:
			{
				resetFlag();
				m_goToAlternateLandPoint = false;

				//get home flight bearing
				m_flightInHomeBearing = getFlightCurrentBearing();

				//get home gimbal bearing, In home, the gimbal head is align with the flight,
				//but the yaw maybe is different with the flight yaw
				m_gimbalInHomeBearing = getGimbalAnglesYaw();

				//this time, the gimbal reset, the head is align with flight head
				m_differenceGimbalYawAndFlightYaw = m_flightInHomeBearing - m_gimbalInHomeBearing;

				//get home position
				T_DjiFcSubscriptionPositionFused position;
				m_djiFlightHandler->getFlightPosition(position, messageTimestamp);

				//radian to degree
				m_flightInHomePosition.latitude = position.latitude * 180 / DJI_PI;
				m_flightInHomePosition.longitude = position.longitude * 180 / DJI_PI;

				//ignor this height
				m_flightInHomePosition.altitude = 0;
				m_flightInHomePosition.visibleSatelliteNumber = position.visibleSatelliteNumber;

				m_djiFlightHandler->getAltitudeBarometer(m_groundAltitudeBarometer, messageTimestamp);

				writeLogFile("m_groundAltitudeBarometer: %f, m_flightInHomeBearing:%f, m_gimbalInHomeBearing:%f\n",
									m_groundAltitudeBarometer, m_flightInHomeBearing, m_gimbalInHomeBearing);

				break;

			}

			case DJI_FC_SUBSCRIPTION_FLIGHT_STATUS_ON_GROUND:
			{
				resetFlag();
				m_goToAlternateLandPoint = false;
				break;
			}

			case DJI_FC_SUBSCRIPTION_FLIGHT_STATUS_IN_AIR:
				break;

			default:
				break;
		}

		//flight control mode change:41-->11-->14-->15-->12-->33->6
		switch(m_flightCurrentMode)
		{
			//mode 0: stablize mode
			case DJI_FC_SUBSCRIPTION_DISPLAY_MODE_MANUAL_CTRL:
				resetFlag();
				break;
			
			//mode 1: althold mode
			case DJI_FC_SUBSCRIPTION_DISPLAY_MODE_ATTITUDE:
				resetFlag();
				break;
			
			//mode 6: loiter or poshold mode, psdk control flight in this mode.
			case DJI_FC_SUBSCRIPTION_DISPLAY_MODE_P_GPS:
				break;

			//mode 11: copter take off to 1 meter high
			case DJI_FC_SUBSCRIPTION_DISPLAY_MODE_AUTO_TAKEOFF:
				resetFlag();
				break;

			//mode 12:copter land
			case DJI_FC_SUBSCRIPTION_DISPLAY_MODE_AUTO_LANDING:
				assistLand();
				break;

			//mode 14: auto execute the waypoint
			case DJI_FC_SUBSCRIPTION_DISPLAY_MODE_RESERVED_14:
				resetFlag();
				break;

			//mode 15: rtl with the current height
			case DJI_FC_SUBSCRIPTION_DISPLAY_MODE_NAVI_GO_HOME:
				resetFlag();
				break;

			//mode 17: controled by SDK API
			case DJI_FC_SUBSCRIPTION_DISPLAY_MODE_NAVI_SDK_CTRL:
				break;

			//mode 33: force copter land
			case DJI_FC_SUBSCRIPTION_DISPLAY_MODE_FORCE_AUTO_LANDING:
				resetFlag();
				break;

			//mode 41: motor start rotating, idling status  on the ground
			case DJI_FC_SUBSCRIPTION_DISPLAY_MODE_ENGINE_START:
				resetFlag();
				break;

			default:
				resetFlag();
            	break;
		}

		sleepMs(1000);
	}

	m_djiFlightHandler->setAssistLandSystemStatus(assistLandStartError);
}

hbAssistLand assistLand;
